/******************************************************************************
 * @file     vcom_serial.c
 * @brief    NANO100 series USBD driver Sample file
 * @version  2.0.0
 * @date     17, Dec, 2014
 *
 * @note
 * Copyright (C) 2014 Nuvoton Technology Corp. All rights reserved.
 ******************************************************************************/

/*!<Includes */
#include <string.h>
#include "Nano100Series.h"
#include "common.h"
#include "hg-hmd.h"

#define USBD_GET_EP_FLAG2()          ((uint32_t)(USBD->EPSTS2))

#define HG_HMD_CMD_GSENSOR_START	(0x00)
#define HG_HMD_CMD_GSENSOR_STOP		(0x01)
#define HG_HMD_CMD_GSENSOR_POLL		(0x02)

#define HG_HMD_CMD_GYRO_START		(0x00)
#define HG_HMD_CMD_GYRO_STOP		(0x01)
#define HG_HMD_CMD_GYRO_POLL		(0x02)

#define HG_HMD_CMD_MAGNETIC_START	(0x00)
#define HG_HMD_CMD_MAGNETIC_STOP	(0x01)
#define HG_HMD_CMD_MAGNETIC_POLL	(0x02)
#define HG_HMD_CMD_DISP_XFER		(0x03)

#define HG_HMD_CMD_LENGTH_MAX		(63)

#define HG_HMD_CMD_DISP_XFER_SYNC_BEGIN		(0x55)
#define HG_HMD_CMD_DISP_XFER_SYNC_END		(0xAA)

#define HG_HMD_CMD_DISP_XFER_RESULT_OK		(0)
#define HG_HMD_CMD_DISP_XFER_RESULT_ERROR	(-1)

#define HG_HMD_CMD_DISP_XFER_GET_CONTRAST			(0x00)
#define HG_HMD_CMD_DISP_XFER_SET_CONTRAST			(0x01)
#define HG_HMD_CMD_DISP_XFER_GET_BRIGHTNESS			(0x02)
#define HG_HMD_CMD_DISP_XFER_SET_BRIGHTNESS			(0x03)
#define HG_HMD_CMD_DISP_XFER_GET_SATURATION			(0x04)
#define HG_HMD_CMD_DISP_XFER_SET_SATURATION			(0x05)
#define HG_HMD_CMD_DISP_XFER_GET_3D					(0x06)
#define HG_HMD_CMD_DISP_XFER_SET_3D					(0x07)
#define HG_HMD_CMD_DISP_XFER_GET_VIDEO_STATE		(0x08)
#define HG_HMD_CMD_DISP_XFER_GET_VERSION			(0x09)
#define HG_HMD_CMD_DISP_XFER_GET_BRIGHTNESS_OFFSET	(0x0A)
#define HG_HMD_CMD_DISP_XFER_SET_BRIGHTNESS_OFFSET	(0x0B)
#define HG_HMD_CMD_DISP_XFER_GET_BRIGHTNESS_ABS		(0x0C)
#define HG_HMD_CMD_DISP_XFER_SET_BRIGHTNESS_ABS		(0x0D)

#define HG_HMD_PARAM_DISP_3D_NONE			(0x00)
#define HG_HMD_PARAM_DISP_3D_HALF_LR		(0x01)
#define HG_HMD_PARAM_DISP_3D_HALF_TB		(0x02)
#define HG_HMD_PARAM_DISP_3D_FULL			(0x03)

#define HG_HMD_PARAM_DISP_VIDEO_STATE_OFF	(0x00)
#define HG_HMD_PARAM_DISP_VIDEO_STATE_ON	(0x01)

#define FRACTOR_SCALE_4		(10000)

static uint32_t u32RxSize3 = 0;
static uint8_t  u8RxBuf3[64];
static uint32_t u32RxSize5 = 0;
static uint32_t u32RxSize7 = 0;

extern struct hg_config tHgConfig;
int epCount[8] = {0};

void SetContrast(struct contrast *tContrast)
{
	int pk;
	int pb;
	int w, r, g, b;

	info("SetContrast, w : %u, r : %u, g : %u, b : %u\n", 
		tContrast->w, tContrast->r, tContrast->g, tContrast->b);
	
	// y = k * x + b
	pk = (511 - 0) * FRACTOR_SCALE_4 / (100 - 0);
	pb = 0  * FRACTOR_SCALE_4 - pk * 0;
	w = (pk * tContrast->w + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	
	SPI_RW_Reg(1, 0x14, w & 0x000000FF);
	SPI_RW_Reg(2, 0x14, w & 0x000000FF);
	
	pk = (127 - 0) * FRACTOR_SCALE_4 / (100 - 0);
	pb = 0  * FRACTOR_SCALE_4 - pk * 0;
	r = (pk * tContrast->r + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	g = (pk * tContrast->g + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	b = (pk * tContrast->b + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	
	SPI_RW_Reg(1, 0x15, ((w & 0x00000100) >> 1) | (r & 0x0000007F));
	SPI_RW_Reg(2, 0x15, ((w & 0x00000100) >> 1) | (r & 0x0000007F));
	SPI_RW_Reg(1, 0x16, g & 0x0000007F);
	SPI_RW_Reg(2, 0x16, g & 0x0000007F);
	SPI_RW_Reg(1, 0x17, b & 0x0000007F);
	SPI_RW_Reg(2, 0x17, b & 0x0000007F);
}

void GetContrast(struct contrast *tContrast)
{
	int pk;
	int pb;
	int w, r, g, b;
	int tmp;
	
	w = SPI_Read(1, 0x14);
	tmp = SPI_Read(1, 0x15);
	w |= (tmp & 0x00000080) << 1;
	r = tmp & 0x0000007F;
	g = SPI_Read(1, 0x16);
	b = SPI_Read(1, 0x17);
	
	// y = k * x + b
	pk = (100 - 0) * FRACTOR_SCALE_4 / (511 - 0);
	pb = 0  * FRACTOR_SCALE_4 - pk * 0;
	tContrast->w = (pk * w + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	
	pk = (100 - 0) * FRACTOR_SCALE_4 / (127 - 0);
	pb = 0  * FRACTOR_SCALE_4 - pk * 0;
	tContrast->r = (pk * r + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	tContrast->g = (pk * g + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	tContrast->b = (pk * b + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
}

void SetBrightness(struct brightness *tBrightness)
{
	int tmp;
	info("SetBrightness, w : %u, r : %u, g : %u, b : %u\n", 
		tBrightness->w, tBrightness->r, tBrightness->g, tBrightness->b);
	
	if (tHgConfig.brightnessOffset > 0) {
		// left
		SPI_RW_Reg(1, 0x1D, tBrightness->w & 0x000000FF);
		// right
		tmp = (int)tBrightness->w + tHgConfig.brightnessOffset;
		if (tmp < 1) {
			tmp = 1;
		} else if (tmp > 255){
			tmp = 255;
		}
		SPI_RW_Reg(2, 0x1D, tmp & 0x000000FF);
	} else if (tHgConfig.brightnessOffset <= 0){
		// right
		SPI_RW_Reg(2, 0x1D, tBrightness->w & 0x000000FF);
		// left
		tmp = (int)tBrightness->w - tHgConfig.brightnessOffset;
		if (tmp < 1) {
			tmp = 1;
		} else if (tmp > 255){
			tmp = 255;
		}
		SPI_RW_Reg(1, 0x1D, tmp & 0x000000FF);
	}
}

void GetBrightness(struct brightness *tBrightness)
{
	int pk;
	int pb;
	int w;
	
	w = SPI_Read(1, 0x1D);
	tBrightness->w = w;
}

void SetSaturation(struct saturation *tSaturation)
{
	int pk;
	int pb;
	int w, r, g, b;

	info("SetSaturation, w : %u, r : %u, g : %u, b : %u\n", 
		tSaturation->w, tSaturation->r, tSaturation->g, tSaturation->b);
	
	// y = k * x + b
	pk = (255 - 0) * FRACTOR_SCALE_4 / (100 - 0);
	pb = 0  * FRACTOR_SCALE_4 - pk * 0;
	w = (pk * tSaturation->w + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	
	SPI_RW_Reg(1, 0x18, w & 0x000000FF);
	SPI_RW_Reg(2, 0x18, w & 0x000000FF);
	
	pk = (127 - 0) * FRACTOR_SCALE_4 / (100 - 0);
	pb = 0  * FRACTOR_SCALE_4 - pk * 0;
	r = (pk * tSaturation->r + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	g = (pk * tSaturation->g + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	b = (pk * tSaturation->b + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	
	SPI_RW_Reg(1, 0x19, r & 0x0000007F);
	SPI_RW_Reg(2, 0x19, r & 0x0000007F);
	SPI_RW_Reg(1, 0x1A, g & 0x0000007F);
	SPI_RW_Reg(2, 0x1A, g & 0x0000007F);
	SPI_RW_Reg(1, 0x1B, b & 0x0000007F);
	SPI_RW_Reg(2, 0x1B, b & 0x0000007F);
}

void GetSaturation(struct saturation *tSaturation)
{
	int pk;
	int pb;
	int w, r, g, b;
	
	w = SPI_Read(1, 0x18);
	r = SPI_Read(1, 0x19);
	g = SPI_Read(1, 0x1A);
	b = SPI_Read(1, 0x1B);
	
	// y = k * x + b
	pk = (100 - 0) * FRACTOR_SCALE_4 / (255 - 0);
	pb = 0  * FRACTOR_SCALE_4 - pk * 0;
	tSaturation->w = (pk * w + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	
	pk = (100 - 0) * FRACTOR_SCALE_4 / (127 - 0);
	pb = 0  * FRACTOR_SCALE_4 - pk * 0;
	tSaturation->r = (pk * r + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	tSaturation->g = (pk * g + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	tSaturation->b = (pk * b + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
}

void HG_HMD_GsensorRspPoll(void)
{
	int x = 0;
	int y = 0;
	int z = 0;
	uint8_t buf[12];
	uint8_t buf_raw[6];
	
	sensor_spi_read_multi(1, 0x3F, buf_raw, sizeof(buf_raw));
	x = buf_raw[0];
	x |= buf_raw[1] << 8;
	x <<= 16;
	x >>= 16;
	x >>= 4;
	
	y = buf_raw[2];
	y |= buf_raw[3] << 8;
	y <<= 16;
	y >>= 16;
	y >>= 4;
	
	z = buf_raw[4];
	z |= buf_raw[5] << 8;
	z <<= 16;
	z >>= 16;
	z >>= 4;

	z=~z;
	y=~y;

	buf[0] = x & 0xFF;
	buf[1] = (x >> 8) & 0xFF;
	buf[2] = (x >> 16) & 0xFF;
	buf[3] = (x >> 24) & 0xFF;
	
	buf[4 + 0] = y & 0xFF;
	buf[4 + 1] = (y >> 8) & 0xFF;
	buf[4 + 2] = (y >> 16) & 0xFF;
	buf[4 + 3] = (y >> 24) & 0xFF;
	
	buf[8 + 0] = z & 0xFF;
	buf[8 + 1] = (z >> 8) & 0xFF;
	buf[8 + 2] = (z >> 16) & 0xFF;
	buf[8 + 3] = (z >> 24) & 0xFF;

	USBD_MemCopy((uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP4)), (uint8_t *)buf, sizeof(buf));
	USBD_SET_PAYLOAD_LEN(EP4, sizeof(buf));
}

void HG_HMD_GsensorHandleCmd(void)
{
	uint8_t *pu8RxBuf;
	
	if (u32RxSize5) {
		pu8RxBuf = (uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP5));
		
		switch (pu8RxBuf[0]) {
			case HG_HMD_CMD_GSENSOR_START:
				break;
			case HG_HMD_CMD_GSENSOR_STOP:
				break;
			case HG_HMD_CMD_GSENSOR_POLL:
				HG_HMD_GsensorRspPoll();
				break;
			default:
				break;
		}
		u32RxSize5 = 0;
		USBD_SET_PAYLOAD_LEN(EP5, EP5_MAX_PKT_SIZE);
	}
}

void HG_HMD_GyroRspPoll(void)
{
	int x = 0;
	int y = 0;
	int z = 0;
	uint8_t buf[12];
	uint8_t buf_raw[6];

	sensor_spi_read_multi(0, 0x3F, buf_raw, sizeof(buf_raw));
	x = buf_raw[0];
	x |= buf_raw[1] << 8;
	x <<= 16;
	x >>= 16;
	if (x < -32767) {
		x = 0;
	}
	
	y = buf_raw[2];
	y |= buf_raw[3] << 8;
	y <<= 16;
	y >>= 16;
	if (y < -32767) {
		y = 0;
	}
	
	z = buf_raw[4];
	z |= buf_raw[5] << 8;
	z <<= 16;
	z >>= 16;
	if (z < -32767) {
		z = 0;
	}

	z=~z;
	y=~y;

	buf[0] = x & 0xFF;
	buf[1] = (x >> 8) & 0xFF;
	buf[2] = (x >> 16) & 0xFF;
	buf[3] = (x >> 24) & 0xFF;
	
	buf[4 + 0] = y & 0xFF;
	buf[4 + 1] = (y >> 8) & 0xFF;
	buf[4 + 2] = (y >> 16) & 0xFF;
	buf[4 + 3] = (y >> 24) & 0xFF;
	
	buf[8 + 0] = z & 0xFF;
	buf[8 + 1] = (z >> 8) & 0xFF;
	buf[8 + 2] = (z >> 16) & 0xFF;
	buf[8 + 3] = (z >> 24) & 0xFF;

	USBD_MemCopy((uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP6)), (uint8_t *)buf, sizeof(buf));
	USBD_SET_PAYLOAD_LEN(EP6, sizeof(buf));
}

void HG_HMD_GyroHandleCmd(void)
{
	uint8_t *pu8RxBuf;
	
	if (u32RxSize7) {
		pu8RxBuf = (uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP7));
		
		switch (pu8RxBuf[0]) {
			case HG_HMD_CMD_GYRO_START:
				break;
			case HG_HMD_CMD_GYRO_STOP:
				break;
			case HG_HMD_CMD_GYRO_POLL:
				HG_HMD_GyroRspPoll();
				break;
			default:
				break;
		}
		u32RxSize7 = 0;
		USBD_SET_PAYLOAD_LEN(EP7, EP7_MAX_PKT_SIZE);
	}
}

void HG_HMD_MagneticRspPoll(void)
{
	static int x = 0;
	static int y = 0;
	static int z = 0;
	uint8_t buf[12];
	uint8_t buf_raw[6];

	sensor_spi_read_multi(2, 0x42, buf_raw, sizeof(buf_raw));
	x = buf_raw[0];
	x |= buf_raw[1] << 8;
	x <<= 16;
	x >>= 16;
	x >>= 3;
	
	y = buf_raw[2];
	y |= buf_raw[3] << 8;
	y <<= 16;
	y >>= 16;
	y >>= 3;
	
	z = buf_raw[4];
	z |= buf_raw[5] << 8;
	z <<= 16;
	z >>= 16;
	z >>= 1;

	z=~z;
	y=~y;
	
	buf[0] = x & 0xFF;
	buf[1] = (x >> 8) & 0xFF;
	buf[2] = (x >> 16) & 0xFF;
	buf[3] = (x >> 24) & 0xFF;
	
	buf[4 + 0] = y & 0xFF;
	buf[4 + 1] = (y >> 8) & 0xFF;
	buf[4 + 2] = (y >> 16) & 0xFF;
	buf[4 + 3] = (y >> 24) & 0xFF;
	
	buf[8 + 0] = z & 0xFF;
	buf[8 + 1] = (z >> 8) & 0xFF;
	buf[8 + 2] = (z >> 16) & 0xFF;
	buf[8 + 3] = (z >> 24) & 0xFF;

	USBD_MemCopy((uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP2)), (uint8_t *)buf, sizeof(buf));
	USBD_SET_PAYLOAD_LEN(EP2, sizeof(buf));
}

short calcChkSum(uint8_t *buffer, int byteOffset, int byteCount)
{
	short chkSum = 0;
	int byteEnd;
	int i;
	
	byteEnd = byteOffset + byteCount;
	for (i = byteOffset; i < byteEnd; i++) {
		chkSum += (short)(buffer[i] & 0x00FF);
	}
	return chkSum;
}

int pack(uint8_t *packedBuf, int packedCount, uint8_t *payloadBuf, int payloadOffset, int payloadCount) {
	int index;
	short chkSum;
	int i;
	
	if (packedCount < (payloadCount + 6)) {
		return -1;
	}

	index = 0;
	packedBuf[index] = HG_HMD_CMD_DISP_XFER_SYNC_BEGIN;
	index++;
	packedBuf[index] = (payloadCount & 0x0000FF00) >> 8;
	index++;
	packedBuf[index] = (payloadCount & 0x000000FF);
	index++;
	for (i = 0; i < payloadCount; i++, index++) {
		packedBuf[index] = payloadBuf[payloadOffset + i];
	}
	chkSum = calcChkSum(packedBuf, 1, payloadCount + 2);
	packedBuf[index] = (chkSum & 0xFF00) >> 8;
	index++;
	packedBuf[index] = chkSum & 0x00FF;
	index++;
	packedBuf[index] = HG_HMD_CMD_DISP_XFER_SYNC_END;
	index++;

	return index;
}

int unpack(uint8_t *packedBuf, int packedCount, uint8_t *payloadBuf, int payloadOffset, int payloadCount) {
	int index;
	short length;
	short chkSumRecv, chkSumCalc;
	int i;
	
	index = 0;
	if (packedBuf[index] != HG_HMD_CMD_DISP_XFER_SYNC_BEGIN) {
		return -1;
	}
	index++;
	length = (short)((packedBuf[index] & 0x00FF) << 8);
	index++;
	length |= (short)(packedBuf[index] & 0x00FF);
	index++;
	if (packedCount != (length + 6)) {
		return -1;
	}
	if (payloadCount < length) {
		return -1;
	}
	for (i = 0; i < length; i++, index++) {
		payloadBuf[payloadOffset + i] = packedBuf[index];
	}
	chkSumCalc = calcChkSum(packedBuf, 1, length + 2);

	chkSumRecv = (short)((packedBuf[index] & 0x00FF) << 8);
	index++;
	chkSumRecv |= (short)(packedBuf[index] & 0x00FF);
	index++;

	if (chkSumRecv != chkSumCalc) {
		return -1;
	}
	if (packedBuf[index] != HG_HMD_CMD_DISP_XFER_SYNC_END) {
		return -1;
	}

	return length;
}

int HG_HMD_DispRspXferError(uint8_t *sendCmdBuf, int sendCmdCount)
{
	int i;
	
	i = 0;
	sendCmdBuf[i] = HG_HMD_CMD_DISP_XFER_RESULT_ERROR;
	i++;
	
	return i;
}

int HG_HMD_DispRspXferSetContrast(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;
	
	tHgConfig.tContrast.w = recvCmdBuf[1];
	tHgConfig.tContrast.r = recvCmdBuf[2];
	tHgConfig.tContrast.g = recvCmdBuf[3];
	tHgConfig.tContrast.b = recvCmdBuf[4];

	HgConfigWrite(&tHgConfig);
	
	SetContrast(&tHgConfig.tContrast);
	
	i = 0;
	sendCmdBuf[i] = HG_HMD_CMD_DISP_XFER_RESULT_OK;
	i++;
	sendCmdBuf[i] = tHgConfig.tContrast.w;
	i++;
	sendCmdBuf[i] = tHgConfig.tContrast.r;
	i++;
	sendCmdBuf[i] = tHgConfig.tContrast.g;
	i++;
	sendCmdBuf[i] = tHgConfig.tContrast.b;
	i++;
	
	return i;
}

int HG_HMD_DispRspXferGetContrast(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;
	
	i = 0;
	sendCmdBuf[i] = HG_HMD_CMD_DISP_XFER_RESULT_OK;
	i++;
	sendCmdBuf[i] = tHgConfig.tContrast.w;
	i++;
	sendCmdBuf[i] = tHgConfig.tContrast.r;
	i++;
	sendCmdBuf[i] = tHgConfig.tContrast.g;
	i++;
	sendCmdBuf[i] = tHgConfig.tContrast.b;
	i++;
	
	return i;
}

int HG_HMD_DispRspXferSetBrightness(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;
	int pk;
	int pb;
	int tmp;
	
	// y = k * x + b
	pk = (255 - 1) * FRACTOR_SCALE_4 / (100 - 1);
	pb = 1  * FRACTOR_SCALE_4 - pk * 1;
	
	tmp = (pk * recvCmdBuf[1] + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	tHgConfig.tBrightness.w = tmp;
	tmp = (pk * recvCmdBuf[2] + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	tHgConfig.tBrightness.r = tmp;
	tmp = (pk * recvCmdBuf[3] + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	tHgConfig.tBrightness.g = tmp;
	tmp = (pk * recvCmdBuf[4] + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	tHgConfig.tBrightness.b = tmp;
	
	HgConfigWrite(&tHgConfig);

	SetBrightness(&tHgConfig.tBrightness);
	
	// y = k * x + b
	pk = (100 - 1) * FRACTOR_SCALE_4 / (255 - 1);
	pb = 1  * FRACTOR_SCALE_4 - pk * 1;
	
	i = 0;
	sendCmdBuf[i] = HG_HMD_CMD_DISP_XFER_RESULT_OK;
	i++;
	tmp = (pk * tHgConfig.tBrightness.w + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	sendCmdBuf[i] = tmp;
	i++;
	tmp = (pk * tHgConfig.tBrightness.r + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	sendCmdBuf[i] = tmp;
	i++;
	tmp = (pk * tHgConfig.tBrightness.g + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	sendCmdBuf[i] = tmp;
	i++;
	tmp = (pk * tHgConfig.tBrightness.b + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	sendCmdBuf[i] = tmp;
	i++;
	
	return i;
}

int HG_HMD_DispRspXferGetBrightness(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;
	int pk;
	int pb;
	int tmp;
	
	// y = k * x + b
	pk = (100 - 1) * FRACTOR_SCALE_4 / (255 - 1);
	pb = 1  * FRACTOR_SCALE_4 - pk * 1;
	
	i = 0;
	sendCmdBuf[i] = HG_HMD_CMD_DISP_XFER_RESULT_OK;
	i++;
	tmp = (pk * tHgConfig.tBrightness.w + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	sendCmdBuf[i] = tmp;
	i++;
	tmp = (pk * tHgConfig.tBrightness.r + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	sendCmdBuf[i] = tmp;
	i++;
	tmp = (pk * tHgConfig.tBrightness.g + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	sendCmdBuf[i] = tmp;
	i++;
	tmp = (pk * tHgConfig.tBrightness.b + pb + FRACTOR_SCALE_4 / 2) / FRACTOR_SCALE_4;
	sendCmdBuf[i] = tmp;
	i++;
	
	return i;
}

int HG_HMD_DispRspXferSetBrightnessAbs(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;

	tHgConfig.tBrightness.w = recvCmdBuf[1];
	tHgConfig.tBrightness.r = recvCmdBuf[2];
	tHgConfig.tBrightness.g = recvCmdBuf[3];
	tHgConfig.tBrightness.b = recvCmdBuf[4];
	
	HgConfigWrite(&tHgConfig);

	SetBrightness(&tHgConfig.tBrightness);
	
	i = 0;
	sendCmdBuf[i] = HG_HMD_CMD_DISP_XFER_RESULT_OK;
	i++;
	sendCmdBuf[i] = tHgConfig.tBrightness.w;
	i++;
	sendCmdBuf[i] = tHgConfig.tBrightness.r;
	i++;
	sendCmdBuf[i] = tHgConfig.tBrightness.g;
	i++;
	sendCmdBuf[i] = tHgConfig.tBrightness.b;
	i++;
	
	return i;
}

int HG_HMD_DispRspXferGetBrightnessAbs(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;
	
	i = 0;
	sendCmdBuf[i] = HG_HMD_CMD_DISP_XFER_RESULT_OK;
	i++;
	sendCmdBuf[i] = tHgConfig.tBrightness.w;
	i++;
	sendCmdBuf[i] = tHgConfig.tBrightness.r;
	i++;
	sendCmdBuf[i] = tHgConfig.tBrightness.g;
	i++;
	sendCmdBuf[i] = tHgConfig.tBrightness.b;
	i++;
	
	return i;
}

int HG_HMD_DispRspXferSetSaturation(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;
	
	tHgConfig.tSaturation.w = recvCmdBuf[1];
	tHgConfig.tSaturation.r = recvCmdBuf[2];
	tHgConfig.tSaturation.g = recvCmdBuf[3];
	tHgConfig.tSaturation.b = recvCmdBuf[4];
	
	HgConfigWrite(&tHgConfig);

	SetSaturation(&tHgConfig.tSaturation);
	
	i = 0;
	sendCmdBuf[i] = HG_HMD_CMD_DISP_XFER_RESULT_OK;
	i++;
	sendCmdBuf[i] = tHgConfig.tSaturation.w;
	i++;
	sendCmdBuf[i] = tHgConfig.tSaturation.r;
	i++;
	sendCmdBuf[i] = tHgConfig.tSaturation.g;
	i++;
	sendCmdBuf[i] = tHgConfig.tSaturation.b;
	i++;
	
	return i;
}

int HG_HMD_DispRspXferGetSaturation(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;
	
	i = 0;
	sendCmdBuf[i] = HG_HMD_CMD_DISP_XFER_RESULT_OK;
	i++;
	sendCmdBuf[i] = tHgConfig.tSaturation.w;
	i++;
	sendCmdBuf[i] = tHgConfig.tSaturation.r;
	i++;
	sendCmdBuf[i] = tHgConfig.tSaturation.g;
	i++;
	sendCmdBuf[i] = tHgConfig.tSaturation.b;
	i++;
	
	return i;
}

int HG_HMD_DispRspXferSet3D(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;
	uint8_t mode;
	
	mode = recvCmdBuf[1];
	if ((mode != HG_HMD_PARAM_DISP_3D_NONE)
		&& (mode != HG_HMD_PARAM_DISP_3D_HALF_LR)
	) {
		i = 0;
		sendCmdBuf[i] = HG_HMD_CMD_DISP_XFER_RESULT_ERROR;
		i++;	
		goto exit;
	}

	if (mode == HG_HMD_PARAM_DISP_3D_NONE) {
		if (status_3d != MODE_2D) {
			status_3d_to_set = MODE_2D;
		}
	} else if (mode == HG_HMD_PARAM_DISP_3D_HALF_LR) {
		if (status_3d != MODE_3D_FULL) {
			status_3d_to_set = MODE_3D_FULL;
		}
	}
	
	i = 0;
	sendCmdBuf[i] = HG_HMD_CMD_DISP_XFER_RESULT_OK;
	i++;

exit:	
	return i;
}

int HG_HMD_DispRspXferGet3D(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;
	uint8_t mode;
	
	if (status_3d == MODE_2D) {
		mode = HG_HMD_PARAM_DISP_3D_NONE;
	} else if (status_3d == MODE_3D_FULL) {
		mode = HG_HMD_PARAM_DISP_3D_HALF_LR;
	}
	
	i = 0;
	sendCmdBuf[i] = HG_HMD_CMD_DISP_XFER_RESULT_OK;
	i++;
	sendCmdBuf[i] = mode;
	i++;
	
	return i;
}

int HG_HMD_DispRspXferGetVideoState(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;
	uint8_t state;
	
	if (is_video_on) {
		state = HG_HMD_PARAM_DISP_VIDEO_STATE_ON;
	} else {
		state = HG_HMD_PARAM_DISP_VIDEO_STATE_OFF;
	}
	
	i = 0;
	sendCmdBuf[i] = HG_HMD_CMD_DISP_XFER_RESULT_OK;
	i++;
	sendCmdBuf[i] = state;
	i++;
	
	return i;
}

int HG_HMD_DispRspXferGetVersion(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;
	uint8_t state;
	
	i = 0;
	sendCmdBuf[i] = HG_HMD_CMD_DISP_XFER_RESULT_OK;
	i++;
	i += sprintf(sendCmdBuf + i, "%u.%u", VERSION_MAJOR, VERSION_MINOR);
	
	return i;
}

int HG_HMD_DispRspXferSetBrightnessOffset(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;
	uint32_t tmp = 0;

	tmp = recvCmdBuf[1];
	tmp |= recvCmdBuf[2] << 8;
	tmp |= recvCmdBuf[3] << 16;
	tmp |= recvCmdBuf[4] << 24;
	tHgConfig.brightnessOffset = tmp;
	
	HgConfigWrite(&tHgConfig);

	SetBrightness(&tHgConfig.tBrightness);
	
	i = 0;
	sendCmdBuf[i] = HG_HMD_CMD_DISP_XFER_RESULT_OK;
	i++;
	tmp = tHgConfig.brightnessOffset;
	sendCmdBuf[i] = tmp & 0xFF;
	i++;
	sendCmdBuf[i] = (tmp >> 8) & 0xFF;
	i++;
	sendCmdBuf[i] = (tmp >> 16) & 0xFF;
	i++;
	sendCmdBuf[i] = (tmp >> 24) & 0xFF;
	i++;
	
	return i;
}

int HG_HMD_DispRspXferGetBrightnessOffset(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;
	uint32_t tmp = 0;
	
	i = 0;
	sendCmdBuf[i] = HG_HMD_CMD_DISP_XFER_RESULT_OK;
	i++;
	tmp = tHgConfig.brightnessOffset;
	sendCmdBuf[i] = tmp & 0xFF;
	i++;
	sendCmdBuf[i] = (tmp >> 8) & 0xFF;
	i++;
	sendCmdBuf[i] = (tmp >> 16) & 0xFF;
	i++;
	sendCmdBuf[i] = (tmp >> 24) & 0xFF;
	i++;
	
	return i;
}

void HG_HMD_DispRspXfer(void)
{
	uint8_t sendPackedBuf[HG_HMD_CMD_LENGTH_MAX];
	int sendPackedCount;
	uint8_t recvPackedBuf[HG_HMD_CMD_LENGTH_MAX];
	int recvPackedCount;
	uint8_t sendCmdBuf[HG_HMD_CMD_LENGTH_MAX - 6];
	int sendCmdCount;
	uint8_t recvCmdBuf[HG_HMD_CMD_LENGTH_MAX - 6];
	int recvCmdCount;
	int i;
	
	for (i = 0; i < (u32RxSize3 - 1); i++) {
		recvPackedBuf[i] = u8RxBuf3[i + 1];
	}
	
	recvPackedCount = u32RxSize3 - 1;
	recvCmdCount = unpack(recvPackedBuf, recvPackedCount, recvCmdBuf, 0, sizeof(recvCmdBuf));
	sendCmdCount = HG_HMD_DispRspXferError(sendCmdBuf, sizeof(sendCmdBuf));
	//info("Disp xfer cmd : %u\n", recvCmdBuf[0]);
	if (recvCmdCount < 0) {
		
	} else {
		if (recvCmdBuf[0] == HG_HMD_CMD_DISP_XFER_SET_CONTRAST) {
			sendCmdCount = HG_HMD_DispRspXferSetContrast(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
		} else if (recvCmdBuf[0] == HG_HMD_CMD_DISP_XFER_GET_CONTRAST) {
			sendCmdCount = HG_HMD_DispRspXferGetContrast(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
		} else if (recvCmdBuf[0] == HG_HMD_CMD_DISP_XFER_SET_BRIGHTNESS) {
			sendCmdCount = HG_HMD_DispRspXferSetBrightness(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
		} else if (recvCmdBuf[0] == HG_HMD_CMD_DISP_XFER_GET_BRIGHTNESS) {
			sendCmdCount = HG_HMD_DispRspXferGetBrightness(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
		} else if (recvCmdBuf[0] == HG_HMD_CMD_DISP_XFER_SET_SATURATION) {
			sendCmdCount = HG_HMD_DispRspXferSetSaturation(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
		} else if (recvCmdBuf[0] == HG_HMD_CMD_DISP_XFER_GET_SATURATION) {
			sendCmdCount = HG_HMD_DispRspXferGetSaturation(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
		} else if (recvCmdBuf[0] == HG_HMD_CMD_DISP_XFER_SET_3D) {
			sendCmdCount = HG_HMD_DispRspXferSet3D(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
		} else if (recvCmdBuf[0] == HG_HMD_CMD_DISP_XFER_GET_3D) {
			sendCmdCount = HG_HMD_DispRspXferGet3D(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
		} else if (recvCmdBuf[0] == HG_HMD_CMD_DISP_XFER_GET_VIDEO_STATE) {
			sendCmdCount = HG_HMD_DispRspXferGetVideoState(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
		} else if (recvCmdBuf[0] == HG_HMD_CMD_DISP_XFER_GET_VERSION) {
			sendCmdCount = HG_HMD_DispRspXferGetVersion(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
		} else if (recvCmdBuf[0] == HG_HMD_CMD_DISP_XFER_SET_BRIGHTNESS_OFFSET) {
			sendCmdCount = HG_HMD_DispRspXferSetBrightnessOffset(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
		} else if (recvCmdBuf[0] == HG_HMD_CMD_DISP_XFER_GET_BRIGHTNESS_OFFSET) {
			sendCmdCount = HG_HMD_DispRspXferGetBrightnessOffset(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
		} else if (recvCmdBuf[0] == HG_HMD_CMD_DISP_XFER_SET_BRIGHTNESS_ABS) {
			sendCmdCount = HG_HMD_DispRspXferSetBrightnessAbs(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
		} else if (recvCmdBuf[0] == HG_HMD_CMD_DISP_XFER_GET_BRIGHTNESS_ABS) {
			sendCmdCount = HG_HMD_DispRspXferGetBrightnessAbs(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
		}
	}
	
	sendPackedCount = pack(sendPackedBuf, sizeof(sendPackedBuf), sendCmdBuf, 0, sendCmdCount);
	USBD_MemCopy((uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP2)), (uint8_t *)sendPackedBuf, sendPackedCount);
	USBD_SET_PAYLOAD_LEN(EP2, sendPackedCount);
}

void HG_HMD_MagneticHandleCmd(void)
{
	uint8_t *pu8RxBuf;
	int i;
	
	if (u32RxSize3) {
		pu8RxBuf = (uint8_t *)(USBD_BUF_BASE + USBD_GET_EP_BUF_ADDR(EP3));
		for (i = 0; i < u32RxSize3; i++) {
			u8RxBuf3[i] = pu8RxBuf[i];
		}
		
		switch (pu8RxBuf[0]) {
			case HG_HMD_CMD_MAGNETIC_START:
				break;
			case HG_HMD_CMD_MAGNETIC_STOP:
				break;
			case HG_HMD_CMD_MAGNETIC_POLL:
				HG_HMD_MagneticRspPoll();
				break;
			case HG_HMD_CMD_DISP_XFER:
				HG_HMD_DispRspXfer();
				break;
			default:
				break;
		}
		u32RxSize3 = 0;
		USBD_SET_PAYLOAD_LEN(EP3, EP3_MAX_PKT_SIZE);
	}
}

void HMD_TransferData(void)
{
	int i;
	uint32_t u32EpStat;
	uint32_t u32EpStat2;

	static uint32_t u32Ep3StatOld = 0;
	static uint32_t u32Ep5StatOld = 0;
	static uint32_t u32Ep7StatOld = 0;

	uint32_t u32Ep3StatNew;
	uint32_t u32Ep5StatNew;
	uint32_t u32Ep7StatNew;

	u32EpStat = USBD_GET_EP_FLAG();
	u32EpStat2 = USBD_GET_EP_FLAG2();
	
	//info("0x%08X ", u32EpStat);
	//info("0x%08X ", u32EpStat2);
	
	for (i = 0; i < 8; i++) {
		//info("%06d ", epCount[i]);
	}
	//info("\n");

	u32Ep3StatNew = u32EpStat & USBD_EPSTS_EPSTS3_Msk;
	if (u32Ep3StatNew != u32Ep3StatOld) {
		u32Ep3StatOld = u32Ep3StatNew;
		if ((u32Ep3StatNew == (0x02 << USBD_EPSTS_EPSTS3_Pos))
			|| (u32Ep3StatNew == (0x06 << USBD_EPSTS_EPSTS3_Pos)))
		{
			u32RxSize3 = USBD_GET_PAYLOAD_LEN(EP3);
			HG_HMD_MagneticHandleCmd();
			USBD_CLR_INT_FLAG(USBD_INTSTS_EP3);
		}
	}

	u32Ep5StatNew = u32EpStat & USBD_EPSTS_EPSTS5_Msk;
	if (u32Ep5StatNew != u32Ep5StatOld) {
		u32Ep5StatOld = u32Ep5StatNew;
		if ((u32Ep5StatNew == (0x02 << USBD_EPSTS_EPSTS5_Pos))
			|| (u32Ep5StatNew == (0x06 << USBD_EPSTS_EPSTS5_Pos)))
		{
			u32RxSize5 = USBD_GET_PAYLOAD_LEN(EP5);
			HG_HMD_GsensorHandleCmd();
			USBD_CLR_INT_FLAG(USBD_INTSTS_EP5);
		}
	}

	u32Ep7StatNew = u32EpStat & USBD_EPSTS2_EPSTS7_Msk;
	if (u32Ep7StatNew != u32Ep7StatOld) {
		u32Ep7StatOld = u32Ep7StatNew;
		if ((u32Ep7StatNew == (0x02 << USBD_EPSTS2_EPSTS7_Pos))
			|| (u32Ep7StatNew == (0x06 << USBD_EPSTS2_EPSTS7_Pos)))
		{
			u32RxSize7 = USBD_GET_PAYLOAD_LEN(EP7);
			HG_HMD_GyroHandleCmd();
			USBD_CLR_INT_FLAG(USBD_INTSTS_EP7);
		}
	}
}

void EP3_Handler(void)
{
	u32RxSize3 = USBD_GET_PAYLOAD_LEN(EP3);
}

void EP5_Handler(void)
{
	u32RxSize5 = USBD_GET_PAYLOAD_LEN(EP5);
}

void EP7_Handler(void)
{
	u32RxSize7 = USBD_GET_PAYLOAD_LEN(EP7);
}

/*--------------------------------------------------------------------------*/
void USBD_IRQHandler(void)
{
	int i;
    uint32_t u32IntSts = USBD_GET_INT_FLAG();
    uint32_t u32State = USBD_GET_BUS_STATE();

	for (i = 0; i < 8; i++) 

{
		if (u32IntSts & (1UL << (16 + i))) {
			epCount[i]++;
		}
	}
//------------------------------------------------------------------
    if (u32IntSts & USBD_INTSTS_FLDET) {
        // Floating detect
        USBD_CLR_INT_FLAG(USBD_INTSTS_FLDET);

        if (USBD_IS_ATTACHED()) {
            /* USB Plug In */
            USBD_ENABLE_USB();
        } else {
            /* USB Un-plug */
            USBD_DISABLE_USB();
        }
    }

//------------------------------------------------------------------
    if (u32IntSts & USBD_INTSTS_BUS) {
        /* Clear event flag */
        USBD_CLR_INT_FLAG(USBD_INTSTS_BUS);

        if (u32State & USBD_STATE_USBRST) {
            /* Bus reset */
            USBD_ENABLE_USB();
            USBD_SwReset();
        }
        if (u32State & USBD_STATE_SUSPEND) {
            /* Enable USB but disable PHY */
            USBD_DISABLE_PHY();
        }
        if (u32State & USBD_STATE_RESUME) {
            /* Enable USB and enable PHY */
            USBD_ENABLE_USB();
        }
    }

//------------------------------------------------------------------
    if (u32IntSts & USBD_INTSTS_USB) {
        // USB event
        if (u32IntSts & USBD_INTSTS_SETUP) {
            // Setup packet
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_SETUP);

            /* Clear the data IN/OUT ready flag of control end-points */
            USBD_STOP_TRANSACTION(EP0);
            USBD_STOP_TRANSACTION(EP1);

            USBD_ProcessSetupPacket();
        }

        // EP events
        if (u32IntSts & USBD_INTSTS_EP0) {
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_EP0);

            // control IN
            USBD_CtrlIn();
        }

        if (u32IntSts & USBD_INTSTS_EP1) {
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_EP1);

            // control OUT
            USBD_CtrlOut();
        }

        if (u32IntSts & USBD_INTSTS_EP2) {
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_EP2);
        }

        if (u32IntSts & USBD_INTSTS_EP3) {
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_EP3);
			// Bulk OUT
            EP3_Handler();
        }

        if (u32IntSts & USBD_INTSTS_EP4) {
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_EP4);
        }

        if (u32IntSts & USBD_INTSTS_EP5) {
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_EP5);
			// Bulk OUT
            EP5_Handler();
        }

        if (u32IntSts & USBD_INTSTS_EP6) {
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_EP6);
        }

        if (u32IntSts & USBD_INTSTS_EP7) {
            /* Clear event flag */
            USBD_CLR_INT_FLAG(USBD_INTSTS_EP7);
            // Bulk OUT
            EP7_Handler();
        }
    }
    /* clear unknown event */
	
    USBD_CLR_INT_FLAG(u32IntSts);
}

/*--------------------------------------------------------------------------*/
/**
  * @brief  USBD Endpoint Config.
  * @param  None.
  * @retval None.
  */
void HMD_Init(void)
{
    /* Init setup packet buffer */
    /* Buffer for setup packet -> [0 ~ 0x7] */
    USBD->BUFSEG = SETUP_BUF_BASE;

    /*****************************************************/
    /* EP0 ==> control IN endpoint, address 0 */
    USBD_CONFIG_EP(EP0, USBD_CFG_CSTALL | USBD_CFG_EPMODE_IN | 0);
    /* Buffer range for EP0 */
    USBD_SET_EP_BUF_ADDR(EP0, EP0_BUF_BASE);

    /* EP1 ==> control OUT endpoint, address 0 */
    USBD_CONFIG_EP(EP1, USBD_CFG_CSTALL | USBD_CFG_EPMODE_OUT | 0);
    /* Buffer range for EP1 */
    USBD_SET_EP_BUF_ADDR(EP1, EP1_BUF_BASE);

    /*****************************************************/
    /* EP2 ==> Bulk IN endpoint, address 1 */
    USBD_CONFIG_EP(EP2, USBD_CFG_EPMODE_IN | BULK_IN_EP_NUM_1);
    /* Buffer offset for EP2 */
    USBD_SET_EP_BUF_ADDR(EP2, EP2_BUF_BASE);

    /* EP3 ==> Bulk Out endpoint, address 1 */
    USBD_CONFIG_EP(EP3, USBD_CFG_EPMODE_OUT | BULK_OUT_EP_NUM_1);
    /* Buffer offset for EP3 */
    USBD_SET_EP_BUF_ADDR(EP3, EP3_BUF_BASE);
    /* trigger receive OUT data */
    USBD_SET_PAYLOAD_LEN(EP3, EP3_MAX_PKT_SIZE);

    /* EP4 ==> Bulk IN endpoint, address 2 */
    USBD_CONFIG_EP(EP4, USBD_CFG_EPMODE_IN | BULK_IN_EP_NUM_2);
    /* Buffer offset for EP4 */
    USBD_SET_EP_BUF_ADDR(EP4, EP4_BUF_BASE);

    /* EP5 ==> Bulk Out endpoint, address 2 */
    USBD_CONFIG_EP(EP5, USBD_CFG_EPMODE_OUT | BULK_OUT_EP_NUM_2);
    /* Buffer offset for EP3 */
    USBD_SET_EP_BUF_ADDR(EP5, EP5_BUF_BASE);
    /* trigger receive OUT data */
    USBD_SET_PAYLOAD_LEN(EP5, EP5_MAX_PKT_SIZE);

	/* EP6 ==> Bulk IN endpoint, address 3 */
    USBD_CONFIG_EP(EP6, USBD_CFG_EPMODE_IN | BULK_IN_EP_NUM_3);
    /* Buffer offset for EP6 */
    USBD_SET_EP_BUF_ADDR(EP6, EP6_BUF_BASE);

    /* EP7==> Bulk Out endpoint, address 3 */
    USBD_CONFIG_EP(EP7, USBD_CFG_EPMODE_OUT | BULK_OUT_EP_NUM_3);
    /* Buffer offset for EP7 */
    USBD_SET_EP_BUF_ADDR(EP7, EP7_BUF_BASE);
    /* trigger receive OUT data */
    USBD_SET_PAYLOAD_LEN(EP7, EP7_MAX_PKT_SIZE);
}


void HMD_ClassRequest(void)
{
    uint8_t buf[8];

    USBD_GetSetupPacket(buf);
}

